#ifndef _marcher_h_
#define _marcher_h_

#include <vector>
#include <list>

#include <math.h>
#include <crtdbg.h>


struct vec2i
{
	int x;
	int y;
	vec2i() : x(0),y(0){}
	vec2i(int _x,int _y) : x(_x), y(_y) {}
};

struct Cell
{
	vec2i pos;
	int typ;
	float dist;

	Cell(int x,int y, int _typ) : pos(x,y),typ(_typ),dist(0){}
};
class CellComp
{
public:
	inline bool operator ()(const Cell*& op1, const Cell*& op2) const
	{
		if (op1->dist>op2->dist) return true;
		return false;
	}
};

enum
{
	Accepted=1,
	Close,
	Trial,
	Far,
};

double Godunov(std::vector<Cell*>& src,int i,int j, int w, int h, float dir);

template <class T>
class Marcher
{

	std::vector<Cell*> cells;

	typedef std::list<Cell*> Closes;
	Closes closes;

public:
	Marcher(){}
	~Marcher(){}
	void dist(const T* inmap,
		int w,
		int h,
		const T& clr1,
		const T& clr2,
		float* dest,
		float dir
		);
};

#define AT(a,b) (b)*w+(a)
#define UP(a,b) __max((b-1),0)*w+(a)
#define DN(a,b) __min((b+1),h-1)*w+(a)
#define RG(a,b) (b)*w+__min((a+1),w-1)
#define LF(a,b) (b)*w+__max((a-1),0)

template <class T>
void Marcher<T>::dist(const T* inmap,int w,int h,const T& clr1,const T& clr2, float* dest, float dir)
{
	int i,j;
	//initialize cells
	cells.resize(w*h);
	for (j=0;j<h;j++)
	{
		for (i=0;i<w;i++)
		{
			int typ;
			if (inmap[AT(i,j)]==clr1)
			{
				typ=Accepted;
			}
			else if (inmap[UP(i,j)]==clr1 ||
				inmap[DN(i,j)]==clr1 ||
				inmap[RG(i,j)]==clr1 ||
				inmap[LF(i,j)]==clr1)
			{
				typ=Close;
			}
			else
			{
				typ=Far;
			}

			cells[AT(i,j)]=new Cell(i,j,typ);
			if (cells[AT(i,j)]->typ==Close)
				cells[AT(i,j)]->dist=0.5*dir;
			else 
				cells[AT(i,j)]->dist=dest[AT(i,j)];
		}
	}

	for (i=0;i<w;i++) cells[AT(i,0)]->typ=Far;
	for (i=0;i<w;i++) cells[AT(i,h-1)]->typ=Far;
	for (i=0;i<h;i++) cells[AT(0,i)]->typ=Far;
	for (i=0;i<h;i++) cells[AT(w-1,i)]->typ=Far;

	//TODO: in Close cells calc precise distance

	for (j=1;j<h-1;j++)
	{
		for (i=1;i<w-1;i++)
		{
			if (cells[AT(i,j)]->typ==Close)
			{
				closes.push_back(cells[AT(i,j)]);
			}
		}
	}
	//---------------------------
	for(;;)
	{
		//	find the smallest in Close set
		int closeCount=0;
		float mind=1000000.0;
		Cell* trial;



		if (closes.empty()) break;

		Closes::iterator it;
		Closes::iterator kill;
		for (it=closes.begin();it!=closes.end();++it)
		{
			if (mind > fabs((*it)->dist) )
			{
				mind=fabs((*it)->dist);
				trial=(*it);
				kill=it;
			}
		}

		//
		trial->typ=Accepted;

		closes.erase(kill);

		i=trial->pos.x;
		j=trial->pos.y;

		if (i<3 || i>w-3 || j<3|| j>h-3) continue;

		Cell* nei[4];
		nei[0]=0;
		nei[1]=0;
		nei[2]=0;
		nei[3]=0;

		if (cells[UP(i,j)]->typ==Far) 
		{
			cells[UP(i,j)]->typ=Close;
			closes.push_back(cells[UP(i,j)]);
		}
		nei[0]=cells[UP(i,j)];
		if (cells[DN(i,j)]->typ==Far)
		{
			cells[DN(i,j)]->typ=Close;
			closes.push_back(cells[DN(i,j)]);
		}
		nei[1]=cells[DN(i,j)];
		if (cells[LF(i,j)]->typ==Far)
		{
			cells[LF(i,j)]->typ=Close;
			closes.push_back(cells[LF(i,j)]);
		}
		nei[2]=cells[LF(i,j)];
		if (cells[RG(i,j)]->typ==Far)
		{
			cells[RG(i,j)]->typ=Close;
			closes.push_back(cells[RG(i,j)]);
		}
		nei[3]=cells[RG(i,j)];

		// recalc distances for all Close cells neighbored by Trial
		for (i=0;i<4;i++)
		{
			if(nei[i]->typ==Close) 
				nei[i]->dist=Godunov(cells,nei[i]->pos.x,nei[i]->pos.y,w,h,dir);
		}
	}

	for (j=0;j<h;j++)
	{
		for (i=0;i<w;i++)
		{
			dest[j*w+i]=cells[j*w+i]->dist;
			delete cells[j*w+i];
		}
	}

	cells.clear();

}

#endif